home *** CD-ROM | disk | FTP | other *** search
/ Aminet 30 / Aminet 30 (1999)(Schatztruhe)[!][Apr 1999].iso / Aminet / gfx / misc / gnuplot-3.7src.lha / gnuplot-3.7src / gnuplot-3.7.lha / gnuplot-3.7 / matrix.c < prev    next >
C/C++ Source or Header  |  1998-11-19  |  7KB  |  270 lines
  1. #ifndef lint
  2. static char *RCSid = "$Id: matrix.c,v 1.15 1998/04/14 00:16:01 drd Exp $";
  3. #endif
  4.  
  5. /*
  6.  *    Matrix algebra, part of
  7.  *
  8.  *    Nonlinear least squares fit according to the
  9.  *    Marquardt-Levenberg-algorithm
  10.  *
  11.  *    added as Patch to Gnuplot (v3.2 and higher)
  12.  *    by Carsten Grammes
  13.  *    Experimental Physics, University of Saarbruecken, Germany
  14.  *
  15.  *    Internet address: cagr@rz.uni-sb.de
  16.  *
  17.  *    Copyright of this module:   Carsten Grammes, 1993
  18.  *
  19.  *    Permission to use, copy, and distribute this software and its
  20.  *    documentation for any purpose with or without fee is hereby granted,
  21.  *    provided that the above copyright notice appear in all copies and
  22.  *    that both that copyright notice and this permission notice appear
  23.  *    in supporting documentation.
  24.  *
  25.  *    This software is provided "as is" without express or implied warranty.
  26.  */
  27.  
  28.  
  29. #include "fit.h"
  30. #include "matrix.h"
  31. #include "stdfn.h"
  32. #include "alloc.h"
  33.  
  34. /*****************************************************************/
  35.  
  36. #define Swap(a,b)   {double temp = (a); (a) = (b); (b) = temp;}
  37. #define WINZIG          1e-30
  38.  
  39.  
  40. /*****************************************************************
  41.     internal prototypes
  42. *****************************************************************/
  43.  
  44. static GP_INLINE int fsign __PROTO((double x));
  45.  
  46. /*****************************************************************
  47.     first straightforward vector and matrix allocation functions
  48. *****************************************************************/
  49. double *vec (n)
  50. int n;
  51. {
  52.     /* allocates a double vector with n elements */
  53.     double *dp;
  54.     if( n < 1 )
  55.     return (double *) NULL;
  56.     dp = (double *) gp_alloc ( n * sizeof(double), "vec");
  57.     return dp;
  58. }
  59.  
  60.  
  61. double **matr (rows, cols)
  62. int rows;
  63. int cols;
  64. {
  65.     /* allocates a double matrix */
  66.  
  67.     register int i;
  68.     register double **m;
  69.  
  70.     if ( rows < 1  ||  cols < 1 )
  71.         return NULL;
  72.     m = (double **) gp_alloc ( rows * sizeof(double *) , "matrix row pointers");
  73.     m[0] = (double *) gp_alloc ( rows * cols * sizeof(double), "matrix elements");
  74.     for ( i = 1; i<rows ; i++ )
  75.         m[i] = m[i-1] + cols;
  76.     return m;
  77. }
  78.  
  79.  
  80. void free_matr (m)
  81. double **m;
  82. {
  83.     free (m[0]);
  84.     free (m);
  85. }
  86.  
  87.  
  88. double *redim_vec (v, n)
  89. double **v;
  90. int n;
  91. {
  92.     if ( n < 1 ) 
  93.       *v = NULL;
  94.     else
  95.       *v = (double *) gp_realloc (*v, n * sizeof(double), "vec");
  96.     return *v;
  97. }
  98.  
  99. void redim_ivec (v, n)
  100. int **v;
  101. int n;
  102. {
  103.     if ( n < 1 ) {
  104.     *v = NULL;
  105.     return;
  106.     }
  107.     *v = (int *) gp_realloc (*v, n * sizeof(int), "ivec");
  108. }
  109.  
  110.  
  111. /* HBB: TODO: is there a better value for 'epsilon'? how to specify
  112.  * 'inline'?  is 'fsign' really not available elsewhere? use
  113.  * row-oriented version (p. 309) instead?
  114.  */
  115.  
  116. static GP_INLINE int fsign(x)
  117.   double x;
  118. {
  119.     return( x>0 ? 1 : (x < 0) ? -1 : 0) ;
  120. }
  121.  
  122. /*****************************************************************
  123.  
  124.      Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
  125.      (QR-decomposition). Direct implementation of the algorithm
  126.      presented in H.R.Schwarz: Numerische Mathematik, 'equation'
  127.      number (7.33)
  128.  
  129.      If 'd == NULL', d is not accesed: the routine just computes the QR
  130.      decomposition of C and exits.
  131.  
  132.      If 'want_r == 0', r is not rotated back (\hat{r} is returned
  133.      instead).
  134.  
  135. *****************************************************************/
  136.  
  137. void Givens (C, d, x, r, N, n, want_r)
  138. double **C;
  139. double *d;
  140. double *x;
  141. double *r;
  142. int N;
  143. int n;
  144. int want_r;
  145. {
  146.     int i, j, k;
  147.     double w, gamma, sigma, rho, temp;
  148.     double epsilon = DBL_EPSILON; /* FIXME (?)*/
  149.  
  150. /* 
  151.  * First, construct QR decomposition of C, by 'rotating away'
  152.  * all elements of C below the diagonal. The rotations are
  153.  * stored in place as Givens coefficients rho.
  154.  * Vector d is also rotated in this same turn, if it exists 
  155.  */
  156.     for (j = 0; j<n; j++) 
  157.         for (i = j+1; i<N; i++) 
  158.             if (C[i][j]) {
  159.                 if (fabs(C[j][j])<epsilon*fabs(C[i][j])) { /* find the rotation parameters */
  160.                     w = -C[i][j];
  161.                     gamma = 0;
  162.                     sigma = 1;
  163.                     rho = 1;
  164.         } else {
  165.             w = fsign(C[j][j])*sqrt(C[j][j]*C[j][j] + C[i][j]*C[i][j]);
  166.             if (w == 0)
  167.             Eex3 ( "w = 0 in Givens();  Cjj = %g,  Cij = %g", C[j][j], C[i][j]);
  168.             gamma = C[j][j]/w;
  169.             sigma = -C[i][j]/w;
  170.             rho = (fabs(sigma)<gamma) ? sigma : fsign(sigma)/gamma;
  171.         }
  172.         C[j][j] = w;
  173.         C[i][j] = rho;           /* store rho in place, for later use */
  174.         for (k = j+1; k<n; k++) {   /* rotation on index pair (i,j) */
  175.             temp =    gamma*C[j][k] - sigma*C[i][k];
  176.             C[i][k] = sigma*C[j][k] + gamma*C[i][k];
  177.             C[j][k] = temp;
  178.             
  179.         }
  180.         if (d) {               /* if no d vector given, don't use it */
  181.             temp = gamma*d[j] - sigma*d[i];  /* rotate d */
  182.             d[i] = sigma*d[j] + gamma*d[i];
  183.             d[j] = temp;
  184.             }
  185.         }
  186.     if (!d)               /* stop here if no d was specified */
  187.          return;
  188.  
  189.     for (i = n-1; i >= 0; i--) {   /* solve R*x+d = 0, by backsubstitution */
  190.         double s = d[i];
  191.         r[i] = 0;              /* ... and also set r[i] = 0 for i<n */
  192.         for (k = i+1; k<n; k++) 
  193.             s += C[i][k]*x[k];
  194.     if (C[i][i] == 0)
  195.       Eex ( "Singular matrix in Givens()");
  196.         x[i] = - s / C[i][i];
  197.     }
  198.     for (i = n; i < N; i++) 
  199.         r[i] = d[i];             /* set the other r[i] to d[i] */
  200.         
  201.     if (!want_r)            /* if r isn't needed, stop here */
  202.         return;
  203.         
  204.     /* rotate back the r vector */
  205.     for (j = n-1; j >= 0; j--)
  206.         for (i = N-1; i >= 0; i--) {
  207.             if ((rho = C[i][j]) == 1) { /* reconstruct gamma, sigma from stored rho */
  208.                  gamma = 0;
  209.                  sigma = 1;
  210.             } else if (fabs(rho)<1) {
  211.                 sigma = rho; 
  212.                 gamma = sqrt(1-sigma*sigma);
  213.             } else {
  214.                 gamma = 1/fabs(rho);
  215.                 sigma = fsign(rho)*sqrt(1-gamma*gamma);
  216.             }
  217.         temp = gamma*r[j] + sigma*r[i];    /* rotate back indices (i,j) */
  218.         r[i] = -sigma*r[j] + gamma*r[i];
  219.         r[j] = temp;
  220.     }
  221. }
  222.  
  223.  
  224. /* Given a triangular Matrix R, compute (R^T * R)^(-1), by forward
  225.  * then back substitution
  226.  * 
  227.  * R, I are n x n Matrices, I is for the result. Both must already be
  228.  * allocated.
  229.  * 
  230.  * Will only calculate the lower triangle of I, as it is symmetric 
  231.  */
  232.  
  233. void Invert_RtR ( R, I, n)
  234. double **R;
  235. double **I;
  236. int n;
  237. {
  238.   int i, j, k;
  239.  
  240.   /* fill in the I matrix, and check R for regularity : */
  241.  
  242.   for (i = 0; i<n; i++) {
  243.     for (j = 0; j<i; j++)  /* upper triangle isn't needed */
  244.       I[i][j] = 0;
  245.     I[i][i] = 1;
  246.     if (! R[i][i])
  247.       Eex ("Singular matrix in Invert_RtR")
  248. }
  249.  
  250.   /* Forward substitution: Solve R^T * B = I, store B in place of I */
  251.  
  252.   for (k = 0; k<n; k++) 
  253.     for (i = k; i<n; i++) {  /* upper half needn't be computed */
  254.       double s = I[i][k];
  255.       for (j = k; j<i; j++)  /* for j<k, I[j][k] always stays zero! */
  256.     s -= R[j][i] * I[j][k];
  257.       I[i][k] = s / R[i][i];
  258. }
  259.  
  260.   /* Backward substitution: Solve R * A = B, store A in place of B */
  261.  
  262.   for (k = 0; k<n; k++)
  263.     for (i = n-1; i >= k; i--) {  /* don't compute upper triangle of A */
  264.       double s = I[i][k];
  265.       for (j = i+1; j<n; j++)
  266.     s -= R[i][j] * I[j][k];
  267.       I[i][k] = s / R[i][i]; 
  268.     }
  269. }
  270.